#ifndef __CHASSIS_H
#define __CHASSIS_H

/*---------------C Scope-----------------------------*/
#ifdef __cplusplus
extern "C"
{
#endif


#ifdef __cplusplus
}
#include "Device/motor/Motor.h"
#include "mechanum/Mechanum.h"
#include "Device/icm42688/user/icm42688_base.h"
#include "Algorithm/PID/PID.h"
class Chassis
{
private:
	Motor* M_LF;
	Motor* M_RF;
	Motor* M_LB;
	Motor* M_RB;
	Mechanum resolve;
	Icm42688* icm;

	float x_target;
	float y_target;
	float r_target;
	float yawAngle;


	int rc= 0;//陀螺仪状态
	float rvquat[4];

	float d_yawTarget=0.0f;
	

public:
	Chassis(Motor* _LF,Motor* _RF,Motor* _LB,Motor* _RB,Icm42688* _icm):M_LF(_LF),M_RF(_RF),M_LB(_LB),M_RB(_RB),icm(_icm){};
	PID_Controller yawAngleLoop;
	PID_Controller yawSpeedLoop;

	void Init();
	void Update(float* _x_target,float* _y_target,float* _yawAngle);
	void Adjust();
	void Acuate();
	
};

#endif
#endif
